#include "kalman_filter.h"
#include"math.h"
float rad2deg = 57.29578;                   //弧度到角度的换算系数
float roll_v = 0, pitch_v = 0;              //换算到x,y轴上的角速度
/*定义微分时间*/
float dt = 0.001;        //根据中断频率修改

/*三个状态，先验状态，观测状态，最优估计状态*/
float gyro_roll = 0, gyro_pitch = 0;        //陀螺仪积分计算出的角度，先验状态
float acc_roll = 0, acc_pitch = 0;          //加速度计观测出的角度，观测状态
float k_roll = 0, k_pitch = 0;              //卡尔曼滤波后估计出最优角度，最优估计状态

/*误差协方差矩阵P*/
float e_P[2][2] ={{1,0},{0,1}};             //误差协方差矩阵，这里的e_P既是先验估计的P，也是最后更新的P

/*卡尔曼增益K*/
float k_k[2][2] ={{0,0},{0,0}};  

void KALMAN_set_hz(float hz)
{
    dt=1/hz;
}

void *kalman_calc(acc_raw_data_t acc_data,gyro_raw_data_t gyro_data,float result[])
{
    /*step1:计算先验状态*/
    /*计算x,y轴上的角速度*/
    roll_v = (gyro_data.roll) + ((sin(k_pitch)*sin(k_roll))/cos(k_pitch))*(gyro_data.pitch) + ((sin(k_pitch)*cos(k_roll))/cos(k_pitch))*gyro_data.yaw;//roll轴的角速度
    pitch_v = cos(k_roll)*(gyro_data.pitch) - sin(k_roll)*gyro_data.yaw;//pitch轴的角速度
    gyro_roll = k_roll + dt*roll_v;//先验roll角度
    gyro_pitch = k_pitch + dt*pitch_v;//先验pitch角度

    /*step2:计算先验误差协方差矩阵P*/
    e_P[0][0] = e_P[0][0] + 0.0025;//这里的Q矩阵是一个对角阵且对角元均为0.0025
    e_P[0][1] = e_P[0][1] + 0;
    e_P[1][0] = e_P[1][0] + 0;
    e_P[1][1] = e_P[1][1] + 0.0025;

    /*step3:更新卡尔曼增益K*/
    k_k[0][0] = e_P[0][0]/(e_P[0][0]+0.3);
    k_k[0][1] = 0;
    k_k[1][0] = 0;
    k_k[1][1] = e_P[1][1]/(e_P[1][1]+0.3);

    /*step4:计算最优估计状态*/
    /*观测状态*/
    //roll角度
    acc_roll = atan((acc_data.y) / (acc_data.z))*rad2deg;
    //pitch角度
    acc_pitch = -1*atan((acc_data.x) / sqrt(acc_data.y*acc_data.y + acc_data.z*acc_data.z))*rad2deg;
    //printf("debug: %f,%f\n",acc_roll,acc_pitch);
    /*最优估计状态*/
    k_roll = gyro_roll + k_k[0][0]*(acc_roll - gyro_roll);
    k_pitch = gyro_pitch + k_k[1][1]*(acc_pitch - gyro_pitch);

    /*step5:更新协方差矩阵P*/
    e_P[0][0] = (1 - k_k[0][0])*e_P[0][0];
    e_P[0][1] = 0;
    e_P[1][0] = 0;
    e_P[1][1] = (1 - k_k[1][1])*e_P[1][1];

    result[0]=k_roll;
    result[1]=k_pitch;
}
